#include "ProngsSubsystem.h"
#include "EntechConstants.h"

ProngsSubsystem::ProngsSubsystem()
{
	m_downSolenoid = new TimedSolenoid (c_prongsSolenoidDownChannel);
	m_upSolenoid = new TimedSolenoid(c_prongsSolenoidUpChannel);
	m_pickupOverride = new OperatorButton(c_pickupOverride);
	m_prongsUpButton = new OperatorButton(c_prongsUpButton);
	m_prongsDownButton = new OperatorButton(c_prongsDownButton);
	m_armBackSwitch = new OperatorButton(c_rollerArmBackSwitch);
	m_armDeployedSwitch = new OperatorButton(c_rollerArmDeployedSwitch);
	m_prongsDownLimit = new DigitalInput(c_prongsLimitSwitch);
}

ProngsSubsystem::~ProngsSubsystem()
{
}

void ProngsSubsystem::DoRobotInit(void)
{
	
}

void ProngsSubsystem::DoAutonomousInit(void)
{
	
}

void ProngsSubsystem::DoTeleopInit(void)
{
	
}

void ProngsSubsystem::DoDisabledInit(void)
{
	
}

void ProngsSubsystem::DoAutonomousPeriodic(void)
{
	
}

void ProngsSubsystem::DoTeleopPeriodic(void)
{
	if (m_pickupOverride->GetBool() == true)
	{
		ManualProngs();	
	}
	else
	{
		if (m_prongsUpButton->Get() == OperatorButton::kJustPressed || m_armBackSwitch->Get() == OperatorButton::kJustPressed)
		{
			SetPosition(kUp);
		}
		else if (m_prongsDownButton->Get() == OperatorButton::kJustPressed || m_armDeployedSwitch->Get() == OperatorButton::kJustPressed)
		{
			SetPosition(kDown);
		}
		m_downSolenoid->DoTeleopPeriodic();
		m_upSolenoid->DoTeleopPeriodic();
		
	}
	
	if (m_prongsDownLimit->Get() == false)
	{
		printf("prongs limit is false \n");
	}
	
	SmartDashboard::PutBoolean("Prongs down: ", GetIsProngsDown());
}

void ProngsSubsystem::DoDisabledPeriodic(void)
{
	
}

void ProngsSubsystem::SetPosition(ArmPosition position)
{
	m_position = position;
	if (m_position == kDown)
	{
		m_upSolenoid->Trigger(true);
		m_downSolenoid->Trigger(false);
		printf("prongs down \n");
	}
	else if(m_position == kUp)
	{
		m_downSolenoid->Trigger(true);
		m_upSolenoid->Trigger(false);
		printf("prongs up \n");
	}
}

void ProngsSubsystem::ManualProngs()
{
	if (m_prongsUpButton->Get() == OperatorButton::kJustPressed)
	{
		SetPosition(kUp);
	}
	else if (m_prongsDownButton->Get() == OperatorButton::kJustPressed)
	{
		SetPosition(kDown);
	}
	m_downSolenoid->DoTeleopPeriodic();
	m_upSolenoid->DoTeleopPeriodic();
}

bool ProngsSubsystem::GetIsProngsDown()
{
	if (m_prongsDownLimit->Get() == false)
	{
		return true;
	}
	else
	{
		return false;
	}
}
